/**
 * Copyright (c) 2018-2022, NXOS Development Team
 * SPDX-License-Identifier: Apache-2.0
 * 
 * Contains: start for aarch64
 * 
 * Change Logs:
 * Date           Author            Notes
 * 2022-09-14     JasonHu           Init
 */

#define __ASSEMBLY__

#include <arch/mm.h>
#include <arch/sysregs.h>
#include <arch/pgtable_prot.h>
#include <nx_configure.h>

#ifdef CONFIG_NX_HYPERVISOR_SUPPORT
#include <virt/virt.h>
.extern NX_VirtInit
#endif /* CONFIG_NX_HYPERVISOR_SUPPORT */

.section ".text.start","ax"
.global _Start
.extern __NX_EarlyMain

_Start:
    /* get cpu id from mpidr_el1 register */
    mrs x0, mpidr_el1   /* MPIDR_EL1: Multi-Processor Affinity Register */
    and x0, x0, #15     /* get cpu id */
    msr tpidr_el1, x0   /* TPID_EL1: Thread ID Register */

    mrs x0, tpidr_el1
    cbz x0, .master  /* if cpu id == 0: goto .master */

.hang:
    wfe /* Wait For Event */
    b .hang

.master:
    /* set stack before our code, Define stack pointer for current exception level */
    adr x1, _Start

    /* get current EL */
    mrs x0, CurrentEL   /* CurrentEL Register. bit 2, 3 are Exception level. Others reserved */
    and x0, x0, #12     /* clear reserved bits */

    /* if current EL != 1100b(11 means EL3): goto .in_el2 */
    cmp x0, #12
    bne .in_el2

    /* should never be executed, just for completeness. (EL3) */
    mov x2, #0x5b1
    msr scr_el3, x2             /* SCR_ELn  Secure Configuration Register */
    mov x2, #0x3c9
    msr spsr_el3, x2            /* SPSR_ELn. Saved Program Status Register. 1111001001 */
    adr x2, .in_el2
    msr elr_el3, x2
    eret                            /* Exception Return: from EL3, continue from .in_el2 */

.in_el2:                     /* running at EL2 or EL1 */
    cmp     x0, #4                  /* 0x04  0100 EL1 */
    beq     .in_el1

    /* in EL2 */
    mrs     x0, hcr_el2
    bic     x0, x0, #0xff
    msr     hcr_el2, x0


    msr     sp_el1, x1              /* in EL2, set sp of EL1 to _Start */

    /* enable CNTP for EL1 */
    mrs     x0, cnthctl_el2         /* Counter-timer Hypervisor Control register */
    orr     x0, x0, #3
    msr     cnthctl_el2, x0
    msr     cntvoff_el2, xzr
#ifdef CONFIG_NX_HYPERVISOR_SUPPORT
    
    mov     x0, #(1 << 31)          /* Enable AArch64 in EL1 */
    orr     x0, x0, #(1 << 1)       /* SWIO hardwired on Pi3 */
    msr     hcr_el2, x0
 
    bl NX_VirtInit

#endif /* CONFIG_NX_HYPERVISOR_SUPPORT */

    /* enable AArch64 in EL1 */
    mov     x0, #(1 << 31)          /* AArch64 */
    orr     x0, x0, #(1 << 1)       /* SWIO hardwired on Pi3 */
    msr     hcr_el2, x0
    mrs     x0, hcr_el2
 
    /* change execution level to EL1 */
    mov     x2, #0x3c4
    msr     spsr_el2, x2            /* 1111000100 */
    adr     x2, .in_el1
    msr     elr_el2, x2

    eret                            /* exception return. from EL2. continue from .in_el1 */

.in_el1:
    mov     x9, #0
    mov     sp, x1                  /* in EL1. Set sp to _Start */

    /* setup vectors */
    ldr     x5, =vectors
	msr     vbar_el1, x5
	isb

    /* Set CPACR_EL1 (Architecture Feature Access Control Register) to avoid trap from SIMD or float point instruction */
    mov     x1, #0x00300000         /* Don't trap any SIMD/FP instructions in both EL0 and EL1 */
    msr     cpacr_el1, x1

    mov     x0, #1
    msr     spsel, x0
    adr     x1, _Start
    mov     sp, x1           /* sp_el1 set to _Start */

    mov x0, #0
    b __NX_EarlyMain

    .global HAl_EnableMmu
HAl_EnableMmu:
	tlbi	vmalle1	// Invalidate local TLB
	dsb	nsh

	/*
	  设置MAIR寄存器，用来配置内存属性
	  在页表项中用来设置内存属性
	 */
	ldr	x5, =MAIR(0x00, MT_DEVICE_nGnRnE) | \
		     MAIR(0x04, MT_DEVICE_nGnRE) | \
		     MAIR(0x0c, MT_DEVICE_GRE) | \
		     MAIR(0x44, MT_NORMAL_NC) | \
		     MAIR(0xff, MT_NORMAL) | \
		     MAIR(0xbb, MT_NORMAL_WT)
	msr	mair_el1, x5

	/*
	   这种TCR寄存器，主要设置
	   1. T0SZ： TTBR0页表的管辖大小
	   2. T1SZ： TTBR1页表管辖大小
	   3. TCR_TG0_4K： TTBR0的页表粒度为4KB
	   4. TCR_TG1_4K： TTBR1的页表粒度为4KB
	 */
	ldr	x10, =TCR_TxSZ(VA_BITS) | TCR_TG_FLAGS
	msr	tcr_el1, x10

	ldr x3, =SCTLR_ELx_M

	/*
	   打开MMU
	 */
	msr sctlr_el1, x3
	isb

	/*
	   无效所有的指令高速缓存
	 */
	/* ic iallu */
	dsb nsh
	isb
	ret


.text
.globl Get_Current_El
Get_Current_El:
    MRS     X0, CurrentEL
    CMP     X0, 0xc
    B.EQ    3f
    CMP     X0, 0x8
    B.EQ    2f
    CMP     X0, 0x4
    B.EQ    1f

    LDR     X0, =0
    B       0f
3:
    LDR     X0, =3
    B       0f
2:
    LDR     X0, =2
    B       0f
1:
    LDR     X0, =1
    B       0f
0:
    RET

